import sensor, image, time
from pyb import UART
from pyb import LED


LED(1).on()
LED(2).on()
LED(3).on()


sensor.reset()
sensor.set_pixformat(sensor.RGB565)
sensor.set_framesize(sensor.QQVGA)
#sensor.set_windowing((0,0,160,90))#主车要缩小竖向缩小窗口
sensor.set_auto_whitebal(False)
sensor.set_auto_gain(False)
sensor.skip_frames(time = 2000)



uart=UART(3,9600)
uart.init(9600, bits=8, parity=None, stop=1)



clock = time.clock()


#th1 = (8,10,-3,0,2,4)
#th2 = (10,15,-6,2,-3,10)
#th3 = (6,7,-4,-1,-1,6)

#车B---410
th1 = (21,35,-11,-3,3,13)
th2 = (14,23,-16,-9,5,17)
th3 = (9,11,-10,-6,3,6)

#车A---410
#th1 = (8,13,-6,0,2,10)
#th2 = (7,10,-4,0,1,4)
#th3 = (3,6,-6,-1,-2,6)


#th1 = (10,20,-7,2,-3,13)
#th2 = (10,18,-6,3,-3,11)
#th3 = (10,20,-6,3,-3,15)


while(True):
    clock.tick()
    img = sensor.snapshot().lens_corr(1.8).binary([th1,th2,th3])
    lin = img.get_regression([(100,255)])
    if(lin):
        rho_err = abs(lin.rho())-80

        if(rho_err>127):
            rho_err = 127
        if(rho_err<-128):
            rho_err = -128

        uart.write(bytearray([int(rho_err)]))
        print('%d'%int(rho_err))
    #print(clock.fps())



